#include "Body3D.h"
#include "Rotation.h"
#include "Quaternion.h"
#include "FastOps.h"
#include "Cholesky.h"

#include <iomanip>
#define RSIM_SMALL 1.0E-10

using namespace RSIM;

void Body3D::init(){
                
        len_ = 1.0;
        mass_ = 1.0;
        rk_ = 0.5;
        betak_ = 1.0;                
        psi1_.Identity();
        nzeta12_.Identity();
        c_nbetak_.Zeros();
        psi2_.Zeros();            
        psi3_.Zeros();
        
        PSI3_.Zeros();
        
        // Just to have a fully populated inertia matrix to begin with
        inertia_.Diagonal(1.0,2.0,3.0);
        Rotation R2(1,2,3,"Body123");   
        inertia_ = R2*inertia_*(~R2);                
        
        double *r_bf = R_FM_.wPtr();
        const double *rot = R2.Ptr();
        for(int i=0;i<9;++i)
                r_bf[i]=rot[i];
        udot_= -100;
        
        double tmp[3];
        tmp[0] = rk_; tmp[1] = 0.0; tmp[2] = 0.0;
        Mat3x3 rkx(0.0);
        FastOps::formCrossProdMat(tmp, rkx.wPtr());
        
        // Jk1 = Inertia - mass * [rx][rx]
        Jb_ = inertia_ - mass_*rkx*rkx;                
}

void Body3D::updRotationMatrix(const Mat3x3& R_NK){
        double t[9];
        double *nck = n_C_k_.wPtr();
        double *rnm = R_NM_.wPtr();
        const double *rnk = R_NK.Ptr();
        FastOps::mult_mat33_mat33(rnk,
                                  nck,
                                  t);
        for(int i=0;i<9;++i)
                nck[i]=t[i];
        
        FastOps::mult_mat33_mat33(rnk,
                                  rnm,
                                  t);
        for(int i=0;i<9;++i)
                rnm[i]=t[i];
        
        nbetak_[0] = betak_*nck[0];
        nbetak_[1] = betak_*nck[3];
        nbetak_[2] = betak_*nck[6];
                
        nrk_[0] = rk_*nck[0];
        nrk_[1] = rk_*nck[3];
        nrk_[2] = rk_*nck[6];                
}

void Body3D::calcNullSpaceProjectionMatrixRow(const int& nbodies,
                                           const double *pinvjac,
                                           double *null){        
        const double *jac = jac_.Ptr();        
        for(int i=0;i<nbodies;++i){
                double tmp=0;
                for(int j=0;j<6;++j){
                        tmp += jac[j]*pinvjac[6*i+j];                                                
                }                              
                null[id_*nbodies+i] = -tmp;
                if(id_==i)
                        null[id_*nbodies+i] += 1.0;                
        }
                
}

void Body3D::updcbetak(const Body3D *parent){
        const double *nbetak = parent->nbetak_.Ptr();
        const double *par_cbetak = parent->c_nbetak_.Ptr();
        double *vec = c_nbetak_.wPtr();
        double * von = v_on_.wPtr();
        vec[0] = nbetak[0] + par_cbetak[0];
        vec[1] = nbetak[1] + par_cbetak[1];
        vec[2] = nbetak[2] + par_cbetak[2];
        
        von[0]=vec[0];
        von[1]=vec[1];
        von[2]=vec[2];
}

void Body3D::calcvin(const double *on){
        double *vin = v_in_.wPtr();
        const double *von = v_on_.Ptr();
        vin[0] = on[0]-von[0];
        vin[1] = on[1]-von[1];
        vin[2] = on[2]-von[2];
}

void Body3D::calcJ(){        
        double *v = v_in_.wPtr();
        v[0] = v[1] = v[2] = 0.0;
        const double *J = n_C_k_.Ptr();
        double *jac = jac_.wPtr();
        jac[0] = J[2];
        jac[1] = J[5];
        jac[2] = J[8];
        jac[3] = jac[4] = jac[5] = 0.0;
}

void Body3D::updvon(const Body3D& parent){
        double *v = v_on_.wPtr();
        const double *pv = parent.v_on_.Ptr();
        const double *beta = parent.nbetak_.Ptr();
        v[0]=pv[0]+beta[0];
        v[1]=pv[1]+beta[1];
        v[2]=pv[2]+beta[2];
}

void Body3D::updvon(){
        double *v = v_on_.wPtr();        
        v[0]=v[1]=v[2]=0.0;
}

void Body3D::calcJFromvin(){
        const double *beta = v_in_.Ptr();
        const double *J = n_C_k_.Ptr();
        double *jac = jac_.wPtr();
        // skbeta' * N_C_K * P
        jac[0] = J[2];
        jac[1] = J[5];
        jac[2] = J[8];
        
        jac[3] = J[5]*beta[2] - J[8]*beta[1];
        jac[4] = J[8]*beta[0] - J[2]*beta[2];
        jac[5] = J[2]*beta[1] - J[5]*beta[0];
}

void Body3D::calcJ(const Body3D *child){
        double *beta = v_in_.wPtr();
        const double *vc = child->v_in_.Ptr();
        const double *nbetak = nbetak_.Ptr();
        const double *J = n_C_k_.Ptr();
        beta[0] = nbetak[0]+vc[0];
        beta[1] = nbetak[1]+vc[1];
        beta[2] = nbetak[2]+vc[2];
        
        double *jac = jac_.wPtr();
        // skbeta' * N_C_K * P
        jac[0] = J[2];
        jac[1] = J[5];
        jac[2] = J[8];
        
        jac[3] = J[5]*beta[2] - J[8]*beta[1];
        jac[4] = J[8]*beta[0] - J[2]*beta[2];
        jac[5] = J[2]*beta[1] - J[5]*beta[0];
}


void Body3D::calcJTx(const double *x){        
        const double *nrot = n_C_k_.Ptr();
        tau_ = x[0]*nrot[2] + x[1]*nrot[5] + x[2]*nrot[8];
}

void Body3D::calcJTx(const Body3D *child,
                     const double *F){
        const double *v = child->v_in_.Ptr();
        const double *nbetak = nbetak_.Ptr();
        double *beta = v_in_.wPtr();
        beta[0] = nbetak[0] + v[0];
        beta[1] = nbetak[1] + v[1];
        beta[2] = nbetak[2] + v[2];
        
        const double *J = n_C_k_.Ptr();
        tau_ = J[2]*(F[0]-F[4]*beta[2]+F[5]*beta[1])+
               J[5]*(F[1]+F[3]*beta[2]-F[5]*beta[0])+
               J[8]*(F[2]-F[3]*beta[1]+F[4]*beta[0]);
}

void Body3D::updcbetak(const Mat3x3& Rot){
        double t[3];
        double *nb =  c_nbetak_.wPtr();
        double *von = v_on_.wPtr();
        FastOps::mult_mat33_vec3(Rot.Ptr(),
                                 nb,
                                 t);
        nb[0] = t[0];
        nb[1] = t[1];
        nb[2] = t[2];        
        
        FastOps::mult_mat33_vec3(Rot.Ptr(),
                                 von,
                                 t);
        von[0] = t[0];
        von[1] = t[1];
        von[2] = t[2];
}

void Body3D::updRotationMatrix(const bool& updVec){        
        
        double *nrot = n_C_k_.wPtr();
        Rotation::setRotationZAxis(q_,nrot);
        // R_NM_ = n_C_k_*R_FM_;
        FastOps::mult_mat33_mat33(nrot,
                                  R_FM_.Ptr(),
                                  R_NM_.wPtr());        
        
        if(updVec){
                nbetak_[0] = betak_*nrot[0];
                nbetak_[1] = betak_*nrot[3];
                nbetak_[2] = betak_*nrot[6];
                        
                nrk_[0] = rk_*nrot[0];
                nrk_[1] = rk_*nrot[3];
                nrk_[2] = rk_*nrot[6];                
        }
}

void Body3D::updRotationMatrix(const Body3D *parent,const bool& updVec){        
        double rot[9];        
        Rotation::setRotationZAxis(q_,rot);                                

        double *nrot = n_C_k_.wPtr();
        double t[9];
        
        // n_C_k_ = parent->n_C_k_*parent->R_FM_*Mat3x3(rot);
        FastOps::mult_mat33_mat33(parent->R_FM_.Ptr(),
                                  rot,
                                  t);
        FastOps::mult_mat33_mat33(parent->n_C_k_.Ptr(),
                                  t,
                                  nrot);        
        
        // R_NM_ = n_C_k_*R_FM_;
        FastOps::mult_mat33_mat33(nrot,
                                  R_FM_.Ptr(),
                                  R_NM_.wPtr());
        
        if(updVec){
                nbetak_[0] = betak_*nrot[0];
                nbetak_[1] = betak_*nrot[3];
                nbetak_[2] = betak_*nrot[6];
                
                nrk_[0] = rk_*nrot[0];
                nrk_[1] = rk_*nrot[3];
                nrk_[2] = rk_*nrot[6];             
        }
}

void Body3D::updAngularVelocity(){
        double *w = w_.wPtr();
        w[0] = 0.0;
        w[1] = 0.0;
        w[2] = u_;
        
        double *v = v_.wPtr();
        v[0] = 0.0;
        v[1] = 0.0;
        v[2] = 0.0;
        
        double *akt = Akt_.wPtr();
        for(int i=0;i<6;akt[i++]=0.0);       
}

void Body3D::updAngularAndLinearVelocity(const Body3D& root,const Body3D *parent, double *z){        
        double *w = w_.wPtr();        
        const double *pw = root.w_.Ptr();        
        const double *J = root.R_NM_.Ptr();        
        double t[3];
        FastOps::mult_mat33_vec3(J,w,t);
        // Calculate Angular Velocity
        w[0] = t[0] + pw[0];
        w[1] = t[1] + pw[1];
        w[2] = t[2] + pw[2];                
        
        double *v = v_.wPtr();
        if(parent){
                
                // Calculate linear velocity        
                FastOps::mult_mat33_vec3(J,v,t);
                v[0] = t[0]+z[0];
                v[1] = t[1]+z[1];
                v[2] = t[2]+z[2];        
                
                FastOps::cross(pw,c_nbetak_.Ptr(),t);        
                v[0]+=t[0];
                v[1]+=t[1];
                v[2]+=t[2];
        }
        else{
                v[0] = z[0];
                v[1] = z[1];
                v[2] = z[2];
        }
}

void Body3D::updAngularVelocity(const Body3D& root){        
        double *w = w_.wPtr();
        const double *pw = root.w_.Ptr();
        const double *pv = root.v_.Ptr();
        const double *J = root.R_NM_.Ptr();        
        double t[6],z[3];
        FastOps::mult_mat33_vec3(J,w,t);
        // Calculate Angular Velocity
        w[0] = t[0] + pw[0];
        w[1] = t[1] + pw[1];
        w[2] = t[2] + pw[2];
        
        const double *cb = c_nbetak_.Ptr();                
        const double *nbeta = root.nbetak_.Ptr();
        z[0] = nbeta[0] + cb[0];
        z[1] = nbeta[1] + cb[1];
        z[2] = nbeta[2] + cb[2];
        
        double *v = v_.wPtr();
        FastOps::cross(pw,
                       z,
                       t+3);
        FastOps::mult_mat33_vec3(J,v,t);        
        v[0] = t[0] + t[3] + pv[0];
        v[1] = t[1] + t[4] + pv[1];
        v[2] = t[2] + t[5] + pv[2];       
}

void Body3D::updAngularVelocity(const Body3D *parent,const bool& calcAkt){
        double *w = w_.wPtr();
        double *v = v_.wPtr();        
        const double *pw = parent->w_.Ptr();
        const double *pv = parent->v_.Ptr();        
        const double *J = parent->R_NM_.Ptr();
        const double *nbetak = parent->nbetak_.Ptr();
        // Calculate Angular Velocity
        w[0] = J[2]*u_ + pw[0];
        w[1] = J[5]*u_ + pw[1];
        w[2] = J[8]*u_ + pw[2];
        // w_ = parent->R_NM_*Vect3(0.0,0.0,u_) + parent->w_;
        
        // Calculate linear velocity
        FastOps::cross(pw,nbetak,v);
        v[0] += pv[0];
        v[1] += pv[1];
        v[2] += pv[2];
        
        if(calcAkt){
                // calculate akt
                double *akt = Akt_.wPtr();
                FastOps::cross(pw,w,akt);
                FastOps::wxwxr(pw,nbetak,akt+3);        
        }
}

void Body3D::n_sweepForwardLillyOrin(const Body3D *parent){
        if(parent){
                
                // xk = pk^T * lambda^{k-1} * pk
                calc_xk(parent->R_NM_.Ptr(),
                        parent->Dat_.Lambda.Ptr(),
                        Dat_.xk);
                
                // L = eye(6) - lambda^{k-1} * pk
                calc_L(parent->R_NM_.Ptr(),
                       parent->Dat_.Lambda.Ptr(),
                       Dat_.xk,
                       Dat_.L.wPtr());
                                
                calcLambdaWithParentLilly(nbetak_.Ptr(),
                                          Dat_.L.Ptr(),
                                          parent->Dat_.Lambda.Ptr(),
                                          eom_.Jk1_.Ptr(),
                                          nrk_.Ptr(),
                                          mass_,
                                          Dat_.Lambda.wPtr());
                                          
                double ak[3],LambdaTimesAk[6];
                // ak = [zeros(3,1);wxwxr]
                FastOps::wxwxr(w_.Ptr(),nbetak_.Ptr(),ak);
                
                // Lambda*ak
                FastOps::mult_mat66_bvec6(Dat_.Lambda.Ptr(),
                                          ak,
                                          LambdaTimesAk);
                                                          
                double pdotqdot[3];
                FastOps::cross(parent->w_.Ptr(),
                               w_.Ptr(),
                               pdotqdot);
                               
                // fk = Lambda^{k-1} * PdotQdot - p^{k-1}
                double fk[6];
                FastOps::mult_mat66_tvec6(parent->Dat_.Lambda.Ptr(), pdotqdot, fk);
                const double *pp = parent->Dat_.p.Ptr();
                for(int i=0;i<6;++i)
                        fk[i]-=pp[i];
                
                double LkfkFk[6];
                calc_Lfk_pFk(Dat_.L.Ptr(),
                             fk,
                             eom_.dFk1_.Ptr(),
                             LkfkFk);
                
                double isklkfkfk[6];
                // inv(Sk)*(Lk*fk + Fk)
                FastOps::spatial_inverse_shift_vec6(    LkfkFk,
                                                        nbetak_.Ptr(),
                                                        isklkfkfk);
                double *p = Dat_.p.wPtr();
                for(int i=0;i<6;++i)
                        p[i] = -LambdaTimesAk[i]-isklkfkfk[i];
                             
        }
        else{
                // inv(Sk)*(I + L*Lambda)*inv(transpose(Sk))
                calcLambdaNoParentLilly(nbetak_.Ptr(),
                                        eom_.Jk1_.Ptr(),
                                        lambda_null,
                                        Dat_.Lambda.wPtr());
                
                double t[12],z[3];                
                // inv(Sk)*Fk
                FastOps::spatial_inverse_shift_vec6(    eom_.dFk1_.Ptr(),
                                                        nbetak_.Ptr(),
                                                        t);
                // ak = [zeros(3,1);wxwxr]
                FastOps::wxwxr(w_.Ptr(),nbetak_.Ptr(),z);
                
                // Lambda*ak
                FastOps::mult_mat66_bvec6(Dat_.Lambda.Ptr(),
                                          z,
                                          t+6);
                double * p = Dat_.p.wPtr();
                // p = -(lambda*ak + inv(Sk)*F)
                for(int i=0;i<6;++i)
                        p[i]=-t[i]-t[i+6];
        }        
                
}

void Body3D::calc_Lfk_pFk(      const double *A,
                                const double *f,
                                const double *F,
                                double *LkfkFk){
        
        LkfkFk[0]=A[0]*f[0] + A[1]*f[1] + A[2]*f[2] + F[0];
        LkfkFk[1]=A[6]*f[0] + A[7]*f[1] + A[8]*f[2] + F[1];
        LkfkFk[2]=A[12]*f[0] + A[13]*f[1] + A[14]*f[2] + F[2];
        LkfkFk[3]=A[18]*f[0] + A[19]*f[1] + A[20]*f[2] + F[3] + f[3];
        LkfkFk[4]=A[24]*f[0] + A[25]*f[1] + A[26]*f[2] + F[4] + f[4];
        LkfkFk[5]=A[30]*f[0] + A[31]*f[1] + A[32]*f[2] + F[5] + f[5];                          
}

void Body3D::calcLambdaWithParentLilly( const double *beta,
                                        const double *A,
                                        const double *Q,
                                        const double *z,
                                        const double *r,
                                        const double& m,
                                        double *L){
        double t[129];
        
        t[2]=m*r[1];
        t[3]=A[25]*Q[11];
        t[4]=A[26]*Q[17];
        t[5]=A[24]*Q[5];
        t[6]=Q[29]+t[3]+t[4]+t[5];
        t[7]=beta[2]*t[6];
        t[8]=A[31]*Q[11];
        t[9]=A[32]*Q[17];
        t[10]=A[30]*Q[5];
        t[11]=Q[35]+m+t[10]+t[8]+t[9];
        t[12]=A[1]*Q[11];
        t[13]=A[2]*Q[17];
        t[14]=A[0]*Q[5];
        t[45]=beta[1]*t[11];
        t[15]=t[12]+t[13]+t[14]+t[2]-t[45]+t[7];
        t[16]=A[31]*Q[10];
        t[17]=A[32]*Q[16];
        t[18]=A[30]*Q[4];
        t[19]=Q[34]+t[16]+t[17]+t[18];
        t[20]=A[25]*Q[10];
        t[21]=A[26]*Q[16];
        t[22]=A[24]*Q[4];
        t[23]=Q[28]+m+t[20]+t[21]+t[22];
        t[24]=beta[2]*t[23];
        t[25]=A[1]*Q[10];
        t[26]=A[2]*Q[16];
        t[27]=A[0]*Q[4];
        t[43]=m*r[2];
        t[44]=beta[1]*t[19];
        t[28]=t[24]+t[25]+t[26]+t[27]-t[43]-t[44];
        t[29]=A[26]*Q[15];
        t[30]=A[24]*Q[3];
        t[31]=A[25]*Q[9];
        t[32]=Q[27]+t[29]+t[30]+t[31];
        t[33]=beta[2]*t[32];
        t[34]=A[32]*Q[15];
        t[35]=A[30]*Q[3];
        t[36]=A[31]*Q[9];
        t[37]=Q[33]+t[34]+t[35]+t[36];
        t[38]=A[0]*Q[3];
        t[39]=A[2]*Q[15];
        t[40]=A[1]*Q[9];
        t[42]=beta[1]*t[37];
        t[41]=t[33]+t[38]+t[39]+t[40]-t[42];
        t[46]=m*r[0];
        t[47]=A[30]*Q[0];
        t[48]=A[32]*Q[12];
        t[49]=A[31]*Q[6];
        t[50]=Q[30]+t[2]+t[47]+t[48]+t[49];
        t[51]=A[19]*Q[11];
        t[52]=A[20]*Q[17];
        t[53]=A[18]*Q[5];
        t[54]=Q[23]+t[51]+t[52]+t[53];
        t[55]=beta[0]*t[11];
        t[56]=A[7]*Q[11];
        t[57]=A[8]*Q[17];
        t[58]=A[6]*Q[5];
        t[87]=beta[2]*t[54];
        t[59]=-t[46]+t[55]+t[56]+t[57]+t[58]-t[87];
        t[60]=A[30]*Q[1];
        t[61]=A[32]*Q[13];
        t[62]=A[31]*Q[7];
        t[63]=beta[0]*t[37];
        t[64]=A[20]*Q[15];
        t[65]=A[18]*Q[3];
        t[66]=A[19]*Q[9];
        t[67]=Q[21]+m+t[64]+t[65]+t[66];
        t[68]=A[6]*Q[3];
        t[69]=A[8]*Q[15];
        t[70]=A[7]*Q[9];
        t[85]=beta[2]*t[67];
        t[71]=t[43]+t[63]+t[68]+t[69]+t[70]-t[85];
        t[72]=A[32]*Q[14];
        t[73]=A[30]*Q[2];
        t[74]=A[31]*Q[8];
        t[75]=Q[32]+t[72]+t[73]+t[74];
        t[76]=A[19]*Q[10];
        t[77]=A[20]*Q[16];
        t[78]=A[18]*Q[4];
        t[79]=Q[22]+t[76]+t[77]+t[78];
        t[80]=beta[0]*t[19];
        t[81]=A[7]*Q[10];
        t[82]=A[8]*Q[16];
        t[83]=A[6]*Q[4];
        t[86]=beta[2]*t[79];
        t[84]=t[80]+t[81]+t[82]+t[83]-t[86];
        t[88]=A[18]*Q[0];
        t[89]=A[20]*Q[12];
        t[90]=A[19]*Q[6];
        t[91]=Q[18]+t[88]+t[89]+t[90];
        t[92]=A[24]*Q[0];
        t[93]=A[26]*Q[12];
        t[94]=A[25]*Q[6];
        t[95]=A[24]*Q[1];
        t[96]=A[26]*Q[13];
        t[97]=A[25]*Q[7];
        t[98]=Q[25]+t[95]+t[96]+t[97];
        t[99]=A[18]*Q[1];
        t[100]=A[20]*Q[13];
        t[101]=A[19]*Q[7];
        t[102]=Q[19]+t[100]+t[101]+t[43]+t[99];
        t[103]=beta[1]*t[54];
        t[104]=A[13]*Q[11];
        t[105]=A[14]*Q[17];
        t[106]=A[12]*Q[5];
        t[128]=beta[0]*t[6];
        t[107]=t[103]+t[104]+t[105]+t[106]-t[128];
        t[108]=beta[1]*t[79];
        t[109]=A[13]*Q[10];
        t[110]=A[14]*Q[16];
        t[111]=A[12]*Q[4];
        t[127]=beta[0]*t[23];
        t[112]=t[108]+t[109]+t[110]+t[111]-t[127]+t[46];
        t[113]=beta[1]*t[67];
        t[114]=A[14]*Q[15];
        t[115]=A[12]*Q[3];
        t[116]=A[13]*Q[9];
        t[126]=beta[0]*t[32];
        t[117]=t[113]+t[114]+t[115]+t[116]-t[126]-t[2];
        t[118]=A[18]*Q[2];
        t[119]=A[20]*Q[14];
        t[120]=A[19]*Q[8];
        t[121]=Q[20]+t[118]+t[119]+t[120]-t[2];
        t[122]=A[26]*Q[14];
        t[123]=A[24]*Q[2];
        t[124]=A[25]*Q[8];
        t[125]=Q[26]+t[122]+t[123]+t[124]+t[46];
        
        L[0]=z[0]+beta[2]*(Q[24]+t[92]+t[93]+t[94]-m*r[2])-beta[1]*t[15]+beta[2]*t[28]-beta[1]*t[50]+A[0]*Q[0]+A[2]*Q[12]+A[1]*Q[6];
        L[1]=z[1]-beta[1]*(Q[31]+t[60]+t[61]+t[62]-m*r[0])+beta[0]*t[15]-beta[2]*t[41]+beta[2]*t[98]+A[0]*Q[1]+A[2]*Q[13]+A[1]*Q[7];
        L[2]=z[2]+beta[2]*t[125]-beta[0]*t[28]+beta[1]*t[41]-beta[1]*t[75]+A[0]*Q[2]+A[2]*Q[14]+A[1]*Q[8];
        L[3]=t[41];
        L[4]=t[28];
        L[5]=t[15];
        L[6]=z[3]+beta[0]*t[50]-beta[1]*t[59]+beta[2]*t[84]-beta[2]*t[91]+A[6]*Q[0]+A[8]*Q[12]+A[7]*Q[6];
        L[7]=z[4]-beta[2]*t[102]+beta[0]*t[59]-beta[2]*t[71]+beta[0]*(Q[31]-t[46]+t[60]+t[61]+t[62])+A[6]*Q[1]+A[8]*Q[13]+A[7]*Q[7];
        L[8]=z[5]-beta[2]*t[121]+beta[1]*t[71]+beta[0]*t[75]-beta[0]*t[84]+A[6]*Q[2]+A[8]*Q[14]+A[7]*Q[8];
        L[9]=t[71];
        L[10]=t[84];
        L[11]=t[59];
        L[12]=z[6]-beta[1]*t[107]+beta[2]*t[112]+beta[1]*t[91]-beta[0]*(Q[24]-t[43]+t[92]+t[93]+t[94])+A[12]*Q[0]+A[14]*Q[12]+A[13]*Q[6];
        L[13]=z[7]+beta[1]*t[102]+beta[0]*t[107]-beta[2]*t[117]-beta[0]*t[98]+A[12]*Q[1]+A[14]*Q[13]+A[13]*Q[7];
        L[14]=z[8]-beta[0]*t[112]+beta[1]*t[117]+beta[1]*t[121]-beta[0]*t[125]+A[14]*Q[14]+A[12]*Q[2]+A[13]*Q[8];
        L[15]=t[117];
        L[16]=t[112];
        L[17]=t[107];
        L[18]=Q[18]-t[103]+t[86]+t[88]+t[89]+t[90];
        L[19]=Q[19]+t[100]+t[101]+t[43]-t[85]+t[99]+beta[0]*t[54];
        L[20]=Q[20]+t[113]+t[118]+t[119]+t[120]-t[2]-beta[0]*t[79];
        L[21]=t[67];
        L[22]=t[79];
        L[23]=t[54];
        L[24]=Q[24]+t[24]-t[43]+t[92]+t[93]+t[94]-beta[1]*t[6];
        L[25]=Q[25]+t[128]-t[33]+t[95]+t[96]+t[97];
        L[26]=Q[26]+t[122]+t[123]+t[124]-t[127]+t[46]+beta[1]*t[32];
        L[27]=t[32];
        L[28]=t[23];
        L[29]=t[6];
        L[30]=Q[30]+t[2]-t[45]+t[47]+t[48]+t[49]+beta[2]*t[19];
        L[31]=Q[31]-t[46]+t[55]+t[60]+t[61]+t[62]-beta[2]*t[37];
        L[32]=Q[32]+t[42]+t[72]+t[73]+t[74]-t[80];
        L[33]=t[37];
        L[34]=t[19];
        L[35]=t[11];
}

void Body3D::calc_L(    const double *p,
                        const double *I,
                        const double& xk,
                        double *M){

        double t[27];
        const double& m = xk;

        t[2]=1.0/m;
        t[3]=I[0]*p[2];
        t[4]=I[1]*p[5];
        t[5]=I[2]*p[8];
        t[6]=t[3]+t[4]+t[5];
        t[7]=I[6]*p[2];
        t[8]=I[7]*p[5];
        t[9]=I[8]*p[8];
        t[10]=t[7]+t[8]+t[9];
        t[11]=I[12]*p[2];
        t[12]=I[13]*p[5];
        t[13]=I[14]*p[8];
        t[14]=t[11]+t[12]+t[13];
        t[15]=I[18]*p[2];
        t[16]=I[19]*p[5];
        t[17]=I[20]*p[8];
        t[18]=t[15]+t[16]+t[17];
        t[19]=I[24]*p[2];
        t[20]=I[25]*p[5];
        t[21]=I[26]*p[8];
        t[22]=t[19]+t[20]+t[21];
        t[23]=I[30]*p[2];
        t[24]=I[31]*p[5];
        t[25]=I[32]*p[8];
        t[26]=t[23]+t[24]+t[25];
        
        M[0]=-p[2]*t[2]*t[6]+1.0;
        M[1]=-p[5]*t[2]*t[6];
        M[2]=-p[8]*t[2]*t[6];
        M[3]=0.0;
        M[4]=0.0;
        M[5]=0.0;
        M[6]=-p[2]*t[2]*t[10];
        M[7]=-p[5]*t[2]*t[10]+1.0;
        M[8]=-p[8]*t[2]*t[10];
        M[9]=0.0;
        M[10]=0.0;
        M[11]=0.0;
        M[12]=-p[2]*t[2]*t[14];
        M[13]=-p[5]*t[2]*t[14];
        M[14]=-p[8]*t[2]*t[14]+1.0;
        M[15]=0.0;
        M[16]=0.0;
        M[17]=0.0;
        M[18]=-p[2]*t[2]*t[18];
        M[19]=-p[5]*t[2]*t[18];
        M[20]=-p[8]*t[2]*t[18];
        M[21]=1.0;
        M[22]=0.0;
        M[23]=0.0;
        M[24]=-p[2]*t[2]*t[22];
        M[25]=-p[5]*t[2]*t[22];
        M[26]=-p[8]*t[2]*t[22];
        M[27]=0.0;
        M[28]=1.0;
        M[29]=0.0;
        M[30]=-p[2]*t[2]*t[26];
        M[31]=-p[5]*t[2]*t[26];
        M[32]=-p[8]*t[2]*t[26];
        M[33]=0.0;
        M[34]=0.0;
        M[35]=1.0;
}

void Body3D::calc_xk(const double *p,
                     const double *I,
                     double &xk){
        xk = p[2]*(I[0]*p[2]+I[6]*p[5]+I[12]*p[8])+p[5]*(I[1]*p[2]+I[7]*p[5]+I[13]*p[8])+p[8]*(I[2]*p[2]+I[8]*p[5]+I[14]*p[8]);
}

void Body3D::calcLambdaNoParentLilly(       const double *r,
                                            const double *z,
                                            const double &lnull,
                                            double *M){
        double t[15];
        const double& m = mass_;
        
        t[2]=r[1]*r[1];
        t[3]=r[2]*r[2];
        t[4]=m*r[0]*r[1];
        t[5]=lnull*t[3];
        t[6]=r[0]*r[0];
        t[7]=lnull*r[2];
        t[8]=m*r[0]*r[2];
        t[9]=m*r[1]*r[2];
        t[10]=lnull*t[6];
        t[11]=lnull*t[2];
        t[12]=lnull*r[0];
        t[13]=lnull*r[1];
        t[14]=lnull+m;
        
        M[0]=lnull+t[5]+t[11]+z[0]-m*t[2]-m*t[3];
        M[1]=t[4]+z[1]-lnull*r[0]*r[1];
        M[2]=t[8]+z[2]-lnull*r[0]*r[2];
        M[3]=0.0;
        M[4]=t[7];
        M[5]=-lnull*r[1];
        M[6]=t[4]+z[3]-lnull*r[0]*r[1];
        M[7]=lnull+t[5]+t[10]+z[4]-m*t[3]-m*t[6];
        M[8]=t[9]+z[5]-lnull*r[1]*r[2];
        M[9]=-t[7];
        M[10]=0.0;
        M[11]=t[12];
        M[12]=t[8]+z[6]-lnull*r[0]*r[2];
        M[13]=t[9]+z[7]-lnull*r[1]*r[2];
        M[14]=t[10]+t[11]+z[8]-m*t[2]-m*t[6];
        M[15]=t[13];
        M[16]=-t[12];
        M[17]=0.0;
        M[18]=0.0;
        M[19]=-t[7];
        M[20]=t[13];
        M[21]=t[14];
        M[22]=0.0;
        M[23]=0.0;
        M[24]=t[7];
        M[25]=0.0;
        M[26]=-t[12];
        M[27]=0.0;
        M[28]=t[14];
        M[29]=0.0;
        M[30]=-t[13];
        M[31]=t[12];
        M[32]=0.0;
        M[33]=0.0;
        M[34]=0.0;
        M[35]=t[14];
}

void Body3D::n_calcZeta12Backward(){
        double *z12 = nzeta12_.wPtr();
        const double *beta = nbetak_.Ptr();
        
        z12[4]=-beta[2]; z12[5]=beta[1];        
        z12[9] = beta[2]; z12[11]= -beta[0];
        z12[15]=-beta[1];  z12[16]=beta[0];        
}

void Body3D::n_calcZeta12Backward(const Body3D *child){
                                                                                   
        const double *z = child->nzeta12_.Ptr();
        
        // multiplier matrix for propagating the constraint force
        // at the other end of the chain.
        double *z12 = nzeta12_.wPtr();
                
        // This is just Triang*child(Z12)
        const double *t = child->aba_.Triang_.Ptr();
        
        FastOps::mult_mat66_mat66(t,z,z12);        
}

void Body3D::n_calcZetaForward_Opt(const int& rootP1,
                                   const Body3D *parent,
                                   const bool& calcH2){
        if(parent){
                const double *J = parent->R_NM_.Ptr();
                // udot = m1 * Ak1 + m2 + m3*F^n_2
                double *m1Ptr = m1_.wPtr();
                double *m3Ptr = m3_.wPtr();
                {                        
                        const double *I = Iskbeta_.Ptr();
                        m1Ptr[0]=-itil*(I[0]*J[2]+I[12]*J[8]+I[6]*J[5]);
                        m1Ptr[1]=-itil*(I[1]*J[2]+I[13]*J[8]+I[7]*J[5]);
                        m1Ptr[2]=-itil*(I[2]*J[2]+I[14]*J[8]+I[8]*J[5]);
                        m1Ptr[3]=-itil*(I[3]*J[2]+I[15]*J[8]+I[9]*J[5]);
                        m1Ptr[4]=-itil*(I[4]*J[2]+I[16]*J[8]+I[10]*J[5]);
                        m1Ptr[5]=-itil*(I[5]*J[2]+I[17]*J[8]+I[11]*J[5]);                                                                        
                }
                
                {
                        const double *f = aba_.cFk_.Ptr();                        
                        m2_ = itil*(J[2]*f[0]+J[5]*f[1]+J[8]*f[2]);
                }
                
                {
                        const double *I = nzeta12_.Ptr();                        
                        m3Ptr[0]=itil*(I[0]*J[2]+I[12]*J[8]+I[6]*J[5]);
                        m3Ptr[1]=itil*(I[1]*J[2]+I[13]*J[8]+I[7]*J[5]);
                        m3Ptr[2]=itil*(I[2]*J[2]+I[14]*J[8]+I[8]*J[5]);
                        m3Ptr[3]=itil*(I[3]*J[2]+I[15]*J[8]+I[9]*J[5]);
                        m3Ptr[4]=itil*(I[4]*J[2]+I[16]*J[8]+I[10]*J[5]);
                        m3Ptr[5]=itil*(I[5]*J[2]+I[17]*J[8]+I[11]*J[5]);                                                                        
                }
                
                // A^{k+1} = psi1 * Ak1 + psi2  + psi3 * F^n_2
                double *psi1 = psi1_.wPtr();
                double *psi2 = psi2_.wPtr();
                double *psi3 = psi3_.wPtr();
                ////////////////////////////////////////
                // Calculate psi1
                // psi1 = skbeta' + H*m1_
                {
                        // H*m1
                        for(int i=0;i<6;++i){
                                psi1[i]=J[2]*m1Ptr[i];
                                psi1[6+i]=J[5]*m1Ptr[i];
                                psi1[12+i]=J[8]*m1Ptr[i];
                        }
                        // += skbeta'                        
                        const double *beta = parent->nbetak_.Ptr();
                        psi1[0]+=1;psi1[7]+=1;psi1[14]+=1;
                        psi1[19] = beta[2];
                        psi1[20] = -beta[1];
                        psi1[24] = -beta[2];
                        psi1[26] =  beta[0];
                        psi1[30] = beta[1];
                        psi1[31] = -beta[0];
                }
                ////////////////////////////////////////
                
                ////////////////////////////////////////
                // Calculate psi2
                // psi2 = Akt + H*m2_;
                {
                        const double *A = Akt_.Ptr();
                        psi2[0]=A[0]+m2_*J[2];
                        psi2[1]=A[1]+m2_*J[5];
                        psi2[2]=A[2]+m2_*J[8];
                        psi2[3]=A[3];
                        psi2[4]=A[4];
                        psi2[5]=A[5];
                }
                ////////////////////////////////////////
                
                ////////////////////////////////////////
                // calculate psi3
                // psi3 = H*m3_;
                {
                        for(int i=0;i<6;++i){
                                psi3[i]=J[2]*m3Ptr[i];
                                psi3[6+i]=J[5]*m3Ptr[i];
                                psi3[12+i]=J[8]*m3Ptr[i];
                        }
                }
                ////////////////////////////////////////
                
                if(id_-rootP1){                                                
                        
                        {
                                // PSI1 = psi1 * parent_PSI1
                                const double *p_PSI1 = parent->PSI1_.Ptr();
                                FastOps::mult_mat66_mat66(psi1,p_PSI1,PSI1_.wPtr());
                        }
                        
                        {
                                const double *p_PSI2 = parent->PSI2_.Ptr();
                                // PSI2 = psi1 * parent_PSI2 + psi2
                                double *PSI2 = PSI2_.wPtr();
                                FastOps::mult_mat66_vec6(psi1,p_PSI2,PSI2);
                                for(int i=0;i<6;++i)
                                        PSI2[i]+=psi2[i];
                        }
                        
                        {
                                // PSI3 = psi1 * parent_PSI3 + psi3
                                const double *p_PSI3 = parent->PSI3_.Ptr();
                                double *PSI3 = PSI3_.wPtr();
                                FastOps::mult_mat66_mat66(psi1,p_PSI3,PSI3);
                                for(int i=0;i<18;++i)
                                        PSI3[i]+=psi3[i];
                        }
                }
                else{                                                
                        {
                                double *PSI1 = PSI1_.wPtr();
                                for(int i=0;i<36;++i)
                                        PSI1[i]=psi1[i];
                        }
                        {
                                double *PSI3 = PSI3_.wPtr();
                                for(int i=0;i<18;++i)
                                        PSI3[i]=psi3[i];
                        }                        
                        {
                                double *PSI2 = PSI2_.wPtr();
                                for(int i=0;i<6;++i)
                                        PSI2[i]=psi2[i];
                        }
                }
        }
        else{                
                double *phi11 = dca_.phi11_.wPtr();
                double *phi12 = dca_.phi12_.wPtr();
                double *phi13 = dca_.phi13_.wPtr();                
                Cholesky chol(aba_.Ik3_);                
                chol.inverse(phi11);                                
                
                if(calcH2){
                        // calculate phi12
                        FastOps::mult_mat66_mat66(phi11,nzeta12_.Ptr(),phi12);
                }

                // calculate phi13_
                FastOps::mult_mat66_vec6(phi11,aba_.Fk3_.Ptr(),phi13);                
        }
}
void Body3D::Wx_J_W(const double *w, const double *c, double *p){
        p[0]= -w[0]*(c[3]*w[2] - c[6]*w[1]) - w[1]*(c[4]*w[2] - c[7]*w[1]) - w[2]*(c[5]*w[2] - c[8]*w[1]);
        p[1]=  w[0]*(c[0]*w[2] - c[6]*w[0]) + w[1]*(c[1]*w[2] - c[7]*w[0]) + w[2]*(c[2]*w[2] - c[8]*w[0]);
        p[2]= -w[0]*(c[0]*w[1] - c[3]*w[0]) - w[1]*(c[1]*w[1] - c[4]*w[0]) - w[2]*(c[2]*w[1] - c[5]*w[0]);
}

void Body3D::n_formDCAHandleEquations_Opt(const Vector& gravity){
        double t[28],I[18];
        
        FastOps::basis_shift_mat33(n_C_k_.Ptr(),inertia_.Ptr(),I+9);
        FastOps::inv_mat3x3(I+9,I);
        const double& im = 1/mass_;
        const double *r = nrk_.Ptr();                

        {
                // calculate phi11                
                double *phi11 = dca_.phi11_.wPtr();
                t[2]=I[3]*r[2];
                t[3]=I[8]*r[1];
                t[4]=I[7]*r[1];
                t[9]=I[4]*r[2];
                t[5]=t[4]-t[9];
                t[6]=I[6]*r[1];
                t[7]=t[3]-I[5]*r[2];
                t[8]=t[2]-t[6];
                t[10]=I[0]*r[2];
                t[11]=I[7]*r[0];
                t[12]=I[1]*r[2];
                t[13]=-t[11]+t[12];
                t[14]=I[2]*r[2];
                t[17]=I[8]*r[0];
                t[15]=t[14]-t[17];
                t[18]=I[6]*r[0];
                t[16]=t[10]-t[18];
                t[19]=I[4]*r[0];
                t[20]=I[2]*r[1];
                t[26]=I[1]*r[1];
                t[21]=t[19]-t[26];
                t[22]=I[5]*r[0];
                t[23]=I[3]*r[0];
                t[24]=t[20]-t[22];
                t[25]=t[23]-I[0]*r[1];
                
                phi11[0]=I[0];
                phi11[1]=I[1];
                phi11[2]=I[2];
                phi11[3]=t[20]-I[1]*r[2];
                phi11[4]=t[10]-I[2]*r[0];
                phi11[5]=-I[0]*r[1]+I[1]*r[0];
                phi11[6]=I[3];
                phi11[7]=I[4];
                phi11[8]=I[5];
                phi11[9]=-I[4]*r[2]+I[5]*r[1];
                phi11[10]=t[2]-I[5]*r[0];
                phi11[11]=t[19]-I[3]*r[1];
                phi11[12]=I[6];
                phi11[13]=I[7];
                phi11[14]=I[8];
                phi11[15]=t[3]-I[7]*r[2];
                phi11[16]=I[6]*r[2]-I[8]*r[0];
                phi11[17]=t[11]-I[6]*r[1];
                phi11[18]=-t[2]+t[6];
                phi11[19]=t[5];
                phi11[20]=t[7];
                phi11[21]=im-r[2]*t[5]+r[1]*t[7];
                phi11[22]=-r[0]*t[7]-r[2]*t[8];
                phi11[23]=r[1]*t[8]+r[0]*(t[4]-t[9]);
                phi11[24]=t[16];
                phi11[25]=t[13];
                phi11[26]=t[15];
                phi11[27]=r[1]*t[15]-r[2]*t[13];
                phi11[28]=im-r[0]*t[15]+r[2]*t[16];
                phi11[29]=r[0]*t[13]-r[1]*t[16];
                phi11[30]=t[25];
                phi11[31]=t[21];
                phi11[32]=-t[20]+t[22];
                phi11[33]=-r[1]*t[24]-r[2]*t[21];
                phi11[34]=r[0]*t[24]+r[2]*t[25];
                phi11[35]=im-r[1]*t[25]+r[0]*(t[19]-t[26]);
        }               
        
        {
                // calculate phi12
                double *phi12 = dca_.phi12_.wPtr();
                t[2]=I[6]*r[1];
                t[3]=I[4]*r[2];
                t[4]=I[7]*r[1];
                t[5]=I[8]*r[1];
                t[8]=I[5]*r[2];
                t[6]=t[5]-t[8];
                t[9]=I[3]*r[2];
                t[7]=t[2]-t[9];
                t[10]=t[3]-t[4];
                t[11]=I[1]*r[2];
                t[12]=I[8]*r[0];
                t[19]=I[7]*r[0];
                t[13]=t[11]-t[19];
                t[14]=I[2]*r[2];
                t[15]=-t[12]+t[14];
                t[16]=I[0]*r[2];
                t[18]=I[6]*r[0];
                t[17]=t[16]-t[18];
                t[20]=I[0]*r[1];
                t[21]=I[5]*r[0];
                t[22]=I[4]*r[0];
                t[25]=I[2]*r[1];
                t[23]=t[21]-t[25];
                t[24]=I[3]*r[0];
                t[26]=t[20]-t[24];
                t[27]=t[22]-I[1]*r[1];
                
                phi12[0]=I[0];
                phi12[1]=I[1];
                phi12[2]=I[2];
                phi12[3]=t[11]-I[2]*r[1];
                phi12[4]=-I[0]*r[2]+I[2]*r[0];
                phi12[5]=t[20]-I[1]*r[0];
                phi12[6]=I[3];
                phi12[7]=I[4];
                phi12[8]=I[5];
                phi12[9]=t[3]-I[5]*r[1];
                phi12[10]=t[21]-I[3]*r[2];
                phi12[11]=I[3]*r[1]-I[4]*r[0];
                phi12[12]=I[6];
                phi12[13]=I[7];
                phi12[14]=I[8];
                phi12[15]=I[7]*r[2]-I[8]*r[1];
                phi12[16]=t[12]-I[6]*r[2];
                phi12[17]=t[2]-I[7]*r[0];
                phi12[18]=t[7];
                phi12[19]=-t[3]+t[4];
                phi12[20]=t[6];
                phi12[21]=im-r[2]*t[10]-r[1]*t[6];
                phi12[22]=-r[2]*t[7]+r[0]*(t[5]-t[8]);
                phi12[23]=r[0]*t[10]+r[1]*(t[2]-t[9]);
                phi12[24]=t[17];
                phi12[25]=t[13];
                phi12[26]=t[15];
                phi12[27]=-r[1]*t[15]+r[2]*t[13];
                phi12[28]=im+r[0]*t[15]-r[2]*t[17];
                phi12[29]=-r[0]*t[13]+r[1]*t[17];
                phi12[30]=-t[20]+t[24];
                phi12[31]=t[27];
                phi12[32]=t[23];
                phi12[33]=-r[1]*t[23]+r[2]*t[27];
                phi12[34]=r[2]*t[26]+r[0]*(t[21]-t[25]);
                phi12[35]=im-r[0]*t[27]-r[1]*t[26];
        }         
        {
                // calculate phi21
                double *phi21 = dca_.phi21_.wPtr();
                const double *phi11 = dca_.phi11_.Ptr();
                for(int i=0;i<18;++i)
                        phi21[i]=phi11[i];
                for(int i=18;i<36;++i)
                        phi21[i]=-phi11[i];
                phi21[21]+=2*im;
                phi21[28]+=2*im;
                phi21[35]+=2*im;
        }        
        {
                // calculate phi22
                double *phi22 = dca_.phi22_.wPtr();
                const double *phi12 = dca_.phi12_.Ptr();
                for(int i=0;i<18;++i)
                        phi22[i]=phi12[i];
                for(int i=18;i<36;++i)
                        phi22[i]=-phi12[i];
                phi22[21]+=2*im;
                phi22[28]+=2*im;
                phi22[35]+=2*im;
        }        
        double tb[6],f[3];
        const double *grav = gravity.Ptr();
        Wx_J_W(w_.Ptr(),I+9,tb);        
        
        tb[3]=grav[0];tb[4]=grav[1];tb[5]=grav[2];
                
        FastOps::mult_mat33_vec3(I,tb,f);                
        tb[0]=-f[0];tb[1]=-f[1];tb[2]=-f[2];                        
        
        double *phi13 = dca_.phi13_.wPtr();
        double *phi23 = dca_.phi23_.wPtr();
        {
                double mr[] = {-r[0], -r[1], -r[2]};        
                FastOps::transpose_skbeta_x_vec6(mr,tb,phi13);
                FastOps::transpose_skbeta_x_vec6(r,tb,phi23);                                                
                FastOps::wxwxr(w_.Ptr(),mr,f);
        }        
        phi13[3] += f[0];
        phi13[4] += f[1];
        phi13[5] += f[2];
        
        phi23[3] -= f[0];
        phi23[4] -= f[1];
        phi23[5] -= f[2];        
}

void Body3D::n_formInboardEOM(const Vect3& gravity){
      
        const double *grav = gravity.Ptr();
        
        // Calculate J in Newtonian frame
        FastOps::basis_shift_mat33(n_C_k_.Ptr(),
                                   Jb_.Ptr(),
                                   eom_.Jk1_.wPtr());
        
        double *fptr = eom_.dFk1_.wPtr();

        fptr[0]=0.0;fptr[1]=0.0;fptr[2]=0.0;
        fptr[3]=0.0;fptr[4]=0.0;fptr[5]=0.0;        
        
        fptr[3] += mass_*grav[0];
        fptr[4] += mass_*grav[1];
        fptr[5] += mass_*grav[2];
        
        FastOps::cross(nrk_.Ptr(),fptr+3,fptr);                                        
        
        double top[3];
        Wx_J_W(w_.Ptr(),eom_.Jk1_.Ptr(),top);                
        
        double bot[3];
        FastOps::wxwxr(w_.Ptr(),nrk_.Ptr(),bot);                
                
        fptr[0] -= top[0];
        fptr[1] -= top[1];
        fptr[2] -= top[2];
        
        fptr[3] -= mass_*bot[0];
        fptr[4] -= mass_*bot[1];
        fptr[5] -= mass_*bot[2];
}

void Body3D::n_formInboardEOM(const Body3D *parent, const Vect3& gravity){
        
        // Calculate Akt
        const double *pw = parent->w_.Ptr();
        const double *p_nbetak = parent->nbetak_.Ptr();
        double *akt = Akt_.wPtr();
        FastOps::cross(pw,w_.Ptr(),akt);
        FastOps::wxwxr(pw,p_nbetak,akt+3);        
        
        const double *grav = gravity.Ptr();
        
        // Calculate J in Newtonian frame
        FastOps::basis_shift_mat33(n_C_k_.Ptr(),
                                   Jb_.Ptr(),
                                   eom_.Jk1_.wPtr());
        
        double *fptr = eom_.dFk1_.wPtr();

        fptr[0]=0.0;fptr[1]=0.0;fptr[2]=0.0;
        fptr[3]=0.0;fptr[4]=0.0;fptr[5]=0.0;        
        
        fptr[3] += mass_*grav[0];
        fptr[4] += mass_*grav[1];
        fptr[5] += mass_*grav[2];
        
        FastOps::cross(nrk_.Ptr(),fptr+3,fptr);                                        
        
        double top[3];
        Wx_J_W(w_.Ptr(),eom_.Jk1_.Ptr(),top);                
        
        double bot[3];
        FastOps::wxwxr(w_.Ptr(),nrk_.Ptr(),bot);                
                
        fptr[0] -= top[0];
        fptr[1] -= top[1];
        fptr[2] -= top[2];
        
        fptr[3] -= mass_*bot[0];
        fptr[4] -= mass_*bot[1];
        fptr[5] -= mass_*bot[2];
}

void Body3D::n_shiftPhiToOutboardHandle_Opt(const Body3D *root,
                                            const bool& nbody){
        if(nbody){
                const double *beta = nbetak_.Ptr();                
                double m1[36],m2[6],m3[36],wxwxr[3];
                
                FastOps::wxwxr(w_.Ptr(),beta,wxwxr);
                FastOps::transpose_skbeta_x_mat66(beta,PSI1_.Ptr(),m1);
                FastOps::transpose_skbeta_x_vec6(beta,PSI2_.Ptr(),m2);
                FastOps::transpose_skbeta_x_mat66(beta,PSI3_.Ptr(),m3);
                m2[3] += wxwxr[0];m2[4] += wxwxr[1];m2[5] += wxwxr[2];        
                {        
                        // calc phi21                
                        FastOps::mult_mat66_mat66(m1,
                                                root->dca_.phi11_.Ptr(),
                                                dca_.phi21_.wPtr()
                                                );                
                }
                {
                        // calc phi22
                        double *phi22 = dca_.phi22_.wPtr();
                        FastOps::mult_mat66_mat66(m1,
                                                root->dca_.phi12_.Ptr(),
                                                phi22);
                        for(int i=0;i<36;++i)
                                phi22[i]+= m3[i];
                }
                
                {
                        double *phi23 = dca_.phi23_.wPtr();
                        FastOps::mult_mat66_vec6(m1,
                                                root->dca_.phi13_.Ptr(),
                                                phi23);
                        for(int i=0;i<6;++i)
                                phi23[i]+=m2[i];
                }    
        }
        else{
                // this is the case when there is a single body in the system
                const double *beta = nbetak_.Ptr();
                FastOps::transpose_skbeta_x_mat66(beta,dca_.phi11_.Ptr(),dca_.phi21_.wPtr());
                FastOps::transpose_skbeta_x_mat66(beta,dca_.phi12_.Ptr(),dca_.phi22_.wPtr());
                FastOps::transpose_skbeta_x_vec6(beta,dca_.phi13_.Ptr(),dca_.phi23_.wPtr());
                double wxwxr[3];
                FastOps::wxwxr(w_.Ptr(),beta,wxwxr);
                dca_.phi23_.wPtr()[3] += wxwxr[0];
                dca_.phi23_.wPtr()[4] += wxwxr[1];
                dca_.phi23_.wPtr()[5] += wxwxr[2];
                
                
                
                
        }
}

void Body3D::n_substituteForward(const Body3D *parent){
        if(parent){                                
                const double *rot = parent->R_NM_.Ptr();
                const double *beta = parent->nbetak_.Ptr();
                const double *A = parent->eom_.ah1_.Ptr();                                
                // skbeta'*Ak1
                double beta_x_A[6];
                
                double v[3];
                FastOps::transpose_skbeta_x_vec6(beta,A,beta_x_A);                                
                const double *cfk = aba_.cFk_.Ptr();                                
                const double *I = aba_.Ik3_.Ptr();
                
                calc_cFk_01(cfk,I,beta_x_A,v);
                v[0] *= itil;v[1] *= itil;v[2] *= itil;
                double dot_tmp[] = {rot[2],rot[5],rot[8]};
                udot_ = Utils::dotN(dot_tmp,v,3);                                
                
                const double *sakt = Akt_.Ptr();         
                double *Ak1 = eom_.ah1_.wPtr();
                Ak1[0] = sakt[0] + beta_x_A[0] + rot[2]*udot_;
                Ak1[1] = sakt[1] + beta_x_A[1] + rot[5]*udot_;
                Ak1[2] = sakt[2] + beta_x_A[2] + rot[8]*udot_;
                Ak1[3] = sakt[3] + beta_x_A[3];
                Ak1[4] = sakt[4] + beta_x_A[4];
                Ak1[5] = sakt[5] + beta_x_A[5];                             
        }
        else{
                const double *I = aba_.Ik3_.Ptr();
                const double *F = aba_.Fk3_.Ptr();
                udot_ = F[2]/I[14];
                
                double *ak1 = eom_.ah1_.wPtr();
                ak1[0] = 0.0;
                ak1[1] = 0.0;
                ak1[2] = udot_;
                ak1[3] = 0.0; 
                ak1[4] = 0.0; 
                ak1[5] = 0.0;
        }
}

void Body3D::setIandF(double *I,
                      double *F){
        const double *J = eom_.Jk1_.Ptr();                
        const double *r = nrk_.Ptr();
        I[0]=J[0]; 
        I[1]=J[1]; 
        I[2]=J[2];  
        I[3]=0.0;       
        I[4]=-mass_*r[2];
        I[5]=mass_*r[1];
        I[6]=J[3]; 
        I[7]=J[4]; 
        I[8]=J[5];  
        I[9]=mass_*r[2]; 
        I[10]=0.0;      
        I[11]=-mass_*r[0];
        I[12]=J[6];
        I[13]=J[7];
        I[14]=J[8]; 
        I[15]=-mass_*r[1];
        I[16]=mass_*r[0];
        I[17]=0.0;
        I[18]=0.0;
        I[19]=mass_*r[2];
        I[20]=-mass_*r[1];
        I[21]=mass_;
        I[22]=0.0;
        I[23]=0.0;                
        I[24]=-mass_*r[2];
        I[25]=0.0;
        I[26]=mass_*r[0];
        I[27]=0.0;
        I[28]=mass_;
        I[29]=0.0;
        I[30]=mass_*r[1];
        I[31]=-mass_*r[0];
        I[32]=0.0;
        I[33]=0.0;
        I[34]=0.0;
        I[35]=mass_;                                
        
        const double *fk = eom_.dFk1_.Ptr();
        F[0] = fk[0]; F[1] = fk[1]; F[2] = fk[2];
        F[3] = fk[3]; F[4] = fk[4]; F[5] = fk[5];
}

void Body3D::calc_itil(const double *J,
                        const double *I, 
                        double& itil){
        const double& tmp = J[2]*(I[0]*J[2]+I[12]*J[8]+I[6]*J[5])+
                            J[5]*(I[1]*J[2]+I[13]*J[8]+I[7]*J[5])+
                            J[8]*(I[2]*J[2]+I[14]*J[8]+I[8]*J[5]);
        assert(fabs(tmp)>RSIM_SMALL);
        itil = 1.0/tmp;
}

void Body3D::n_triangularizeBackward(const Body3D *parent,
                                     const Body3D *child){                               
        
        double *I = aba_.Ik3_.wPtr();
        double *f = aba_.Fk3_.wPtr();        
        
        setIandF(I,f);
                
        if(child){                                                
                // update the ABI and bias force of this body                
                const double *ik3_child = child->aba_.ik3_child_.Ptr();
                const double *fk3_child = child->aba_.fk3_child_.Ptr();
                for(int j=0;j<36;++j)
                        I[j] += ik3_child[j];
                for(int j=0;j<6;++j)
                        f[j] += fk3_child[j];
        }            
        
        if(parent){                                                                                                
                // p = H'*I*H (new label)
                const double *r_nm = parent->R_NM_.Ptr();
                calc_itil(r_nm,
                          I,
                          itil);                        
                                                
                const double *beta = parent->nbetak_.Ptr();
                double *T = aba_.Triang_.wPtr();
                calcTriang(beta, 
                           I, 
                           r_nm,
                           itil, 
                           T);                
                                
                // calculate Ik3*skbeta'
                double *iskbetat = Iskbeta_.wPtr();
                calc_I_x_transposeskbeta(I, beta, iskbetat);
                
                // calculate Triang*Ik3*skbeta'
                double *ik3c = aba_.ik3_child_.wPtr();
                FastOps::mult_mat66_mat66(T,iskbetat,ik3c);                
                
                // calculate (Fk3-Ik3*Akt)
                // Here
                double *cfk = aba_.cFk_.wPtr();
                const double *akt = Akt_.Ptr();
                calc_cFk(f,I,akt,cfk);                
                                
                // calculate Triang*(Fk3-Ik3*Akt)
                double *fk3_child = aba_.fk3_child_.wPtr();
                FastOps::mult_mat66_vec6(T,cfk,fk3_child);
                
        }// if(parent)                                                                
}

void Body3D::calc_cFk(const double *F, 
                      const double *I, 
                      const double *A, 
                      double *p) const{
        p[0]=F[0]-A[0]*I[0]-A[1]*I[1]-A[2]*I[2]-A[3]*I[3]-A[4]*I[4]-A[5]*I[5];
        p[1]=F[1]-A[4]*I[10]-A[0]*I[6]-A[5]*I[11]-A[1]*I[7]-A[2]*I[8]-A[3]*I[9];
        p[2]=F[2]-A[0]*I[12]-A[1]*I[13]-A[2]*I[14]-A[3]*I[15]-A[4]*I[16]-A[5]*I[17];
        p[3]=F[3]-A[0]*I[18]-A[1]*I[19]-A[2]*I[20]-A[3]*I[21]-A[4]*I[22]-A[5]*I[23];
        p[4]=F[4]-A[0]*I[24]-A[1]*I[25]-A[2]*I[26]-A[3]*I[27]-A[4]*I[28]-A[5]*I[29];
        p[5]=F[5]-A[0]*I[30]-A[1]*I[31]-A[2]*I[32]-A[3]*I[33]-A[4]*I[34]-A[5]*I[35];    
}

void Body3D::calc_cFk_01(const double *F, 
                      const double *I, 
                      const double *A, 
                      double *p) const{
        p[0]=F[0]-A[0]*I[0]-A[1]*I[1]-A[2]*I[2]-A[3]*I[3]-A[4]*I[4]-A[5]*I[5];
        p[1]=F[1]-A[4]*I[10]-A[0]*I[6]-A[5]*I[11]-A[1]*I[7]-A[2]*I[8]-A[3]*I[9];
        p[2]=F[2]-A[0]*I[12]-A[1]*I[13]-A[2]*I[14]-A[3]*I[15]-A[4]*I[16]-A[5]*I[17];        
}

void Body3D::calc_m_cFk(const double *F, const double *I, const double *A, double *p) const{
        p[0]=-(F[0]-A[0]*I[0]-A[1]*I[1]-A[2]*I[2]-A[3]*I[3]-A[4]*I[4]-A[5]*I[5]);
        p[1]=-(F[1]-A[4]*I[10]-A[0]*I[6]-A[5]*I[11]-A[1]*I[7]-A[2]*I[8]-A[3]*I[9]);
        p[2]=-(F[2]-A[0]*I[12]-A[1]*I[13]-A[2]*I[14]-A[3]*I[15]-A[4]*I[16]-A[5]*I[17]);
        p[3]=-(F[3]-A[0]*I[18]-A[1]*I[19]-A[2]*I[20]-A[3]*I[21]-A[4]*I[22]-A[5]*I[23]);
        p[4]=-(F[4]-A[0]*I[24]-A[1]*I[25]-A[2]*I[26]-A[3]*I[27]-A[4]*I[28]-A[5]*I[29]);
        p[5]=-(F[5]-A[0]*I[30]-A[1]*I[31]-A[2]*I[32]-A[3]*I[33]-A[4]*I[34]-A[5]*I[35]);
}

void Body3D::calcTriang_I_tBeta(        const double *A,
                                        const double *I,
                                        const double *beta,
                                        double *ik3c){
        double t[128];
        
        t[2]=A[1]*I[11];
        t[3]=A[2]*I[17];
        t[4]=A[0]*I[5];
        t[5]=A[3]*I[23];
        t[6]=A[4]*I[29];
        t[7]=A[5]*I[35];
        t[8]=t[2]+t[3]+t[4]+t[5]+t[6]+t[7];
        t[9]=A[1]*I[10];
        t[10]=A[2]*I[16];
        t[11]=A[0]*I[4];
        t[12]=A[3]*I[22];
        t[13]=A[4]*I[28];
        t[14]=A[5]*I[34];
        t[15]=t[10]+t[11]+t[12]+t[13]+t[14]+t[9];
        t[16]=A[0]*I[3];
        t[17]=A[2]*I[15];
        t[18]=A[3]*I[21];
        t[19]=A[4]*I[27];
        t[20]=A[5]*I[33];
        t[21]=A[1]*I[9];
        t[22]=t[16]+t[17]+t[18]+t[19]+t[20]+t[21];
        t[23]=A[10]*I[29];
        t[24]=A[11]*I[35];
        t[25]=A[7]*I[11];
        t[26]=A[8]*I[17];
        t[27]=A[6]*I[5];
        t[28]=A[9]*I[23];
        t[29]=t[23]+t[24]+t[25]+t[26]+t[27]+t[28];
        t[30]=A[10]*I[28];
        t[31]=A[11]*I[34];
        t[32]=A[7]*I[10];
        t[33]=A[8]*I[16];
        t[34]=A[6]*I[4];
        t[35]=A[9]*I[22];
        t[36]=t[30]+t[31]+t[32]+t[33]+t[34]+t[35];
        t[37]=A[10]*I[27];
        t[38]=A[11]*I[33];
        t[39]=A[6]*I[3];
        t[40]=A[8]*I[15];
        t[41]=A[9]*I[21];
        t[42]=A[7]*I[9];
        t[43]=t[37]+t[38]+t[39]+t[40]+t[41]+t[42];
        t[44]=A[13]*I[11];
        t[45]=A[14]*I[17];
        t[46]=A[15]*I[23];
        t[47]=A[16]*I[29];
        t[48]=A[17]*I[35];
        t[49]=A[12]*I[5];
        t[50]=t[44]+t[45]+t[46]+t[47]+t[48]+t[49];
        t[51]=A[13]*I[10];
        t[52]=A[14]*I[16];
        t[53]=A[15]*I[22];
        t[54]=A[16]*I[28];
        t[55]=A[17]*I[34];
        t[56]=A[12]*I[4];
        t[57]=t[51]+t[52]+t[53]+t[54]+t[55]+t[56];
        t[58]=A[14]*I[15];
        t[59]=A[15]*I[21];
        t[60]=A[16]*I[27];
        t[61]=A[12]*I[3];
        t[62]=A[17]*I[33];
        t[63]=A[13]*I[9];
        t[64]=t[58]+t[59]+t[60]+t[61]+t[62]+t[63];
        t[65]=A[19]*I[11];
        t[66]=A[20]*I[17];
        t[67]=A[21]*I[23];
        t[68]=A[22]*I[29];
        t[69]=A[23]*I[35];
        t[70]=A[18]*I[5];
        t[71]=t[65]+t[66]+t[67]+t[68]+t[69]+t[70];
        t[72]=A[19]*I[10];
        t[73]=A[20]*I[16];
        t[74]=A[21]*I[22];
        t[75]=A[22]*I[28];
        t[76]=A[18]*I[4];
        t[77]=A[23]*I[34];
        t[78]=t[72]+t[73]+t[74]+t[75]+t[76]+t[77];
        t[79]=A[20]*I[15];
        t[80]=A[18]*I[3];
        t[81]=A[21]*I[21];
        t[82]=A[22]*I[27];
        t[83]=A[23]*I[33];
        t[84]=A[19]*I[9];
        t[85]=t[79]+t[80]+t[81]+t[82]+t[83]+t[84];
        t[86]=A[25]*I[11];
        t[87]=A[26]*I[17];
        t[88]=A[27]*I[23];
        t[89]=A[28]*I[29];
        t[90]=A[29]*I[35];
        t[91]=A[24]*I[5];
        t[92]=t[86]+t[87]+t[88]+t[89]+t[90]+t[91];
        t[93]=A[25]*I[10];
        t[94]=A[26]*I[16];
        t[95]=A[27]*I[22];
        t[96]=A[28]*I[28];
        t[97]=A[29]*I[34];
        t[98]=A[24]*I[4];
        t[99]=t[93]+t[94]+t[95]+t[96]+t[97]+t[98];
        t[100]=A[26]*I[15];
        t[101]=A[27]*I[21];
        t[102]=A[28]*I[27];
        t[103]=A[24]*I[3];
        t[104]=A[29]*I[33];
        t[105]=A[25]*I[9];
        t[106]=t[100]+t[101]+t[102]+t[103]+t[104]+t[105];
        t[107]=A[31]*I[11];
        t[108]=A[32]*I[17];
        t[109]=A[33]*I[23];
        t[110]=A[34]*I[29];
        t[111]=A[35]*I[35];
        t[112]=A[30]*I[5];
        t[113]=t[107]+t[108]+t[109]+t[110]+t[111]+t[112];
        t[114]=A[31]*I[10];
        t[115]=A[32]*I[16];
        t[116]=A[33]*I[22];
        t[117]=A[34]*I[28];
        t[118]=A[35]*I[34];
        t[119]=A[30]*I[4];
        t[120]=t[114]+t[115]+t[116]+t[117]+t[118]+t[119];
        t[121]=A[32]*I[15];
        t[122]=A[33]*I[21];
        t[123]=A[34]*I[27];
        t[124]=A[30]*I[3];
        t[125]=A[35]*I[33];
        t[126]=A[31]*I[9];
        t[127]=t[121]+t[122]+t[123]+t[124]+t[125]+t[126];
        
        ik3c[0]=-beta[2]*t[15]+beta[1]*t[8]+A[0]*I[0]+A[2]*I[12]+A[3]*I[18]+A[4]*I[24]+A[1]*I[6]+A[5]*I[30];
        ik3c[1]=beta[2]*t[22]-beta[0]*t[8]+A[0]*I[1]+A[2]*I[13]+A[3]*I[19]+A[4]*I[25]+A[1]*I[7]+A[5]*I[31];
        ik3c[2]=beta[0]*t[15]-beta[1]*t[22]+A[0]*I[2]+A[2]*I[14]+A[3]*I[20]+A[4]*I[26]+A[5]*I[32]+A[1]*I[8];
        ik3c[3]=t[22];
        ik3c[4]=t[15];
        ik3c[5]=t[8];
        ik3c[6]=beta[1]*t[29]-beta[2]*t[36]+A[10]*I[24]+A[11]*I[30]+A[6]*I[0]+A[8]*I[12]+A[9]*I[18]+A[7]*I[6];
        ik3c[7]=-beta[0]*t[29]+beta[2]*t[43]+A[10]*I[25]+A[11]*I[31]+A[6]*I[1]+A[8]*I[13]+A[9]*I[19]+A[7]*I[7];
        ik3c[8]=beta[0]*t[36]-beta[1]*t[43]+A[10]*I[26]+A[11]*I[32]+A[6]*I[2]+A[8]*I[14]+A[9]*I[20]+A[7]*I[8];
        ik3c[9]=t[43];
        ik3c[10]=t[36];
        ik3c[11]=t[29];
        ik3c[12]=beta[1]*t[50]-beta[2]*t[57]+A[12]*I[0]+A[14]*I[12]+A[15]*I[18]+A[16]*I[24]+A[17]*I[30]+A[13]*I[6];
        ik3c[13]=-beta[0]*t[50]+beta[2]*t[64]+A[12]*I[1]+A[14]*I[13]+A[15]*I[19]+A[16]*I[25]+A[17]*I[31]+A[13]*I[7];
        ik3c[14]=beta[0]*t[57]-beta[1]*t[64]+A[14]*I[14]+A[12]*I[2]+A[15]*I[20]+A[16]*I[26]+A[17]*I[32]+A[13]*I[8];
        ik3c[15]=t[64];
        ik3c[16]=t[57];
        ik3c[17]=t[50];
        ik3c[18]=beta[1]*t[71]-beta[2]*t[78]+A[18]*I[0]+A[20]*I[12]+A[21]*I[18]+A[22]*I[24]+A[23]*I[30]+A[19]*I[6];
        ik3c[19]=-beta[0]*t[71]+beta[2]*t[85]+A[18]*I[1]+A[20]*I[13]+A[21]*I[19]+A[22]*I[25]+A[23]*I[31]+A[19]*I[7];
        ik3c[20]=beta[0]*t[78]-beta[1]*t[85]+A[18]*I[2]+A[20]*I[14]+A[21]*I[20]+A[22]*I[26]+A[23]*I[32]+A[19]*I[8];
        ik3c[21]=t[85];
        ik3c[22]=t[78];
        ik3c[23]=t[71];
        ik3c[24]=beta[1]*t[92]-beta[2]*t[99]+A[24]*I[0]+A[26]*I[12]+A[27]*I[18]+A[28]*I[24]+A[29]*I[30]+A[25]*I[6];
        ik3c[25]=beta[2]*t[106]-beta[0]*t[92]+A[24]*I[1]+A[26]*I[13]+A[27]*I[19]+A[28]*I[25]+A[29]*I[31]+A[25]*I[7];
        ik3c[26]=-beta[1]*t[106]+beta[0]*t[99]+A[26]*I[14]+A[24]*I[2]+A[27]*I[20]+A[28]*I[26]+A[29]*I[32]+A[25]*I[8];
        ik3c[27]=t[106];
        ik3c[28]=t[99];
        ik3c[29]=t[92];
        ik3c[30]=beta[1]*t[113]-beta[2]*t[120]+A[30]*I[0]+A[32]*I[12]+A[33]*I[18]+A[34]*I[24]+A[35]*I[30]+A[31]*I[6];
        ik3c[31]=-beta[0]*t[113]+beta[2]*t[127]+A[30]*I[1]+A[32]*I[13]+A[33]*I[19]+A[34]*I[25]+A[35]*I[31]+A[31]*I[7];
        ik3c[32]=beta[0]*t[120]-beta[1]*t[127]+A[32]*I[14]+A[30]*I[2]+A[33]*I[20]+A[34]*I[26]+A[35]*I[32]+A[31]*I[8];
        ik3c[33]=t[127];
        ik3c[34]=t[120];
        ik3c[35]=t[113];
}

void Body3D::calcTriang(        const double *beta,
                                const double *I,
                                const double *J,
                                const double& im,
                                double *m){
        double t[28];
        t[2]=im;
        t[3]=I[0]*J[2];
        t[4]=I[1]*J[5];
        t[5]=I[2]*J[8];
        t[6]=t[3]+t[4]+t[5];
        t[7]=I[24]*J[2];
        t[8]=I[25]*J[5];
        t[9]=I[26]*J[8];
        t[10]=t[7]+t[8]+t[9];
        t[11]=I[30]*J[2];
        t[12]=I[31]*J[5];
        t[13]=I[32]*J[8];
        t[14]=t[11]+t[12]+t[13];
        t[15]=I[6]*J[2];
        t[16]=I[7]*J[5];
        t[17]=I[8]*J[8];
        t[18]=t[15]+t[16]+t[17];
        t[19]=I[18]*J[2];
        t[20]=I[19]*J[5];
        t[21]=I[20]*J[8];
        t[22]=t[19]+t[20]+t[21];
        t[23]=I[12]*J[2];
        t[24]=I[13]*J[5];
        t[25]=I[14]*J[8];
        t[26]=t[23]+t[24]+t[25];
        
        m[0]=-J[2]*t[2]*t[6]+J[2]*beta[2]*t[10]*t[2]-J[2]*beta[1]*t[14]*t[2]+1.0;
        m[1]=-J[5]*t[2]*t[6]+J[5]*beta[2]*t[10]*t[2]-J[5]*beta[1]*t[14]*t[2];
        m[2]=-J[8]*t[2]*t[6]+J[8]*beta[2]*t[10]*t[2]-J[8]*beta[1]*t[14]*t[2];
        m[3]=0.0;
        m[4]=-beta[2];
        m[5]=beta[1];
        m[6]=-J[2]*t[18]*t[2]+J[2]*beta[0]*t[14]*t[2]-J[2]*beta[2]*t[2]*t[22];
        m[7]=-J[5]*t[18]*t[2]+J[5]*beta[0]*t[14]*t[2]-J[5]*beta[2]*t[2]*t[22]+1.0;
        m[8]=-J[8]*t[18]*t[2]+J[8]*beta[0]*t[14]*t[2]-J[8]*beta[2]*t[2]*t[22];
        m[9]=beta[2];
        m[10]=0.0;
        m[11]=-beta[0];
        m[12]=-J[2]*t[2]*t[26]-J[2]*beta[0]*t[10]*t[2]+J[2]*beta[1]*t[2]*t[22];
        m[13]=-J[5]*t[2]*t[26]-J[5]*beta[0]*t[10]*t[2]+J[5]*beta[1]*t[2]*t[22];
        m[14]=-J[8]*t[2]*t[26]-J[8]*beta[0]*t[10]*t[2]+J[8]*beta[1]*t[2]*t[22]+1.0;
        m[15]=-beta[1];
        m[16]=beta[0];
        m[17]=0.0;
        m[18]=-J[2]*t[2]*t[22];
        m[19]=-J[5]*t[2]*t[22];
        m[20]=-J[8]*t[2]*t[22];
        m[21]=1.0;
        m[22]=0.0;
        m[23]=0.0;
        m[24]=-J[2]*t[10]*t[2];
        m[25]=-J[5]*t[10]*t[2];
        m[26]=-J[8]*t[10]*t[2];
        m[27]=0.0;
        m[28]=1.0;
        m[29]=0.0;
        m[30]=-J[2]*t[14]*t[2];
        m[31]=-J[5]*t[14]*t[2];
        m[32]=-J[8]*t[14]*t[2];
        m[33]=0.0;
        m[34]=0.0;
        m[35]=1.0;
}


void Body3D::calc_phiOneAndTwo( const double *beta,
                                const double *A,
                                const double *J,
                                double *phi){
        phi[0]=A[0]+J[0];
        phi[1]=A[1]+J[1];
        phi[2]=A[2]+J[2];
        phi[3]=A[3]+J[3];
        phi[4]=A[4]+J[4];
        phi[5]=A[5]+J[5];
        phi[6]=A[6]+J[6];
        phi[7]=A[7]+J[7];
        phi[8]=A[8]+J[8];
        phi[9]=A[9]+A[3]*beta[2]-A[6]*beta[1];
        phi[10]=A[10]+A[4]*beta[2]-A[7]*beta[1];
        phi[11]=A[11]+A[5]*beta[2]-A[8]*beta[1];
        phi[12]=A[12]-A[0]*beta[2]+A[6]*beta[0];
        phi[13]=A[13]-A[1]*beta[2]+A[7]*beta[0];
        phi[14]=A[14]-A[2]*beta[2]+A[8]*beta[0];
        phi[15]=A[15]+A[0]*beta[1]-A[3]*beta[0];
        phi[16]=A[16]+A[1]*beta[1]-A[4]*beta[0];
        phi[17]=A[17]+A[2]*beta[1]-A[5]*beta[0];
}

void Body3D::calc_I_x_transposeskbeta(const double *I, const double *beta, double *m){
        m[0]=I[0]-I[4]*beta[2]+I[5]*beta[1];
        m[1]=I[1]+I[3]*beta[2]-I[5]*beta[0];
        m[2]=I[2]-I[3]*beta[1]+I[4]*beta[0];
        m[3]=I[3];
        m[4]=I[4];
        m[5]=I[5];
        m[6]=I[6]-I[10]*beta[2]+I[11]*beta[1];
        m[7]=I[7]-I[11]*beta[0]+I[9]*beta[2];
        m[8]=I[8]+I[10]*beta[0]-I[9]*beta[1];
        m[9]=I[9];
        m[10]=I[10];
        m[11]=I[11];
        m[12]=I[12]-I[16]*beta[2]+I[17]*beta[1];
        m[13]=I[13]+I[15]*beta[2]-I[17]*beta[0];
        m[14]=I[14]-I[15]*beta[1]+I[16]*beta[0];
        m[15]=I[15];
        m[16]=I[16];
        m[17]=I[17];
        m[18]=I[18]-I[22]*beta[2]+I[23]*beta[1];
        m[19]=I[19]+I[21]*beta[2]-I[23]*beta[0];
        m[20]=I[20]-I[21]*beta[1]+I[22]*beta[0];
        m[21]=I[21];
        m[22]=I[22];
        m[23]=I[23];
        m[24]=I[24]-I[28]*beta[2]+I[29]*beta[1];
        m[25]=I[25]+I[27]*beta[2]-I[29]*beta[0];
        m[26]=I[26]-I[27]*beta[1]+I[28]*beta[0];
        m[27]=I[27];
        m[28]=I[28];
        m[29]=I[29];
        m[30]=I[30]-I[34]*beta[2]+I[35]*beta[1];
        m[31]=I[31]+I[33]*beta[2]-I[35]*beta[0];
        m[32]=I[32]-I[33]*beta[1]+I[34]*beta[0];
        m[33]=I[33];
        m[34]=I[34];
        m[35]=I[35];
}